/**
* @file adis16460.h in ADIS16460_Reader
* @author rebeater
* @comment SPI driver for ADIS16460
* Create on 2021/1/29 下午8:06
* @version 1.0
**/

#include "main.h"
#include <adis1646x.h>
#include <stdio.h>
#include <rtcommon.h>
#include "cmsis_os.h"
#include "global_defines.h"

#ifndef IMU_DEVICE
#define IMU_ADIS16460 1
#define IMU_DEVICE IMU_ADIS16460
#warning "IMU_DEVICE is not defined, use default IMU: ADIS16460"
#endif

#define FLASH_CNT 0x00
#define DIAG_STAT 0x02

/* gyro and acce */
#define X_GYRO_LOW 0x04
#define X_GYRO_OUT 0x06
#define Y_GYRO_LOW 0x08
#define Y_GYRO_OUT 0x0A
#define Z_GYRO_LOW 0x0C
#define Z_GYRO_OUT 0x0E

#define X_ACCE_LOW 0x10
#define X_ACCE_OUT 0x12
#define Y_ACCE_LOW 0x14
#define Y_ACCE_OUT 0x16
#define Z_ACCE_LOW 0x18
#define Z_ACCE_OUT 0x1A

#define PI 3.1415926535897932384626
#if IMU_DEVICE == IMU_ADIS16460
#define MSC_CTRL 0x32
#define SYNC_SCAL 0x34
#define DEC_RATE 0x36
#define FLTR_CTRL 0x38
#define GLOB_CMD 0x3E
#define ADIS1646X_WHO_AM_I 0x56
#define ADIS1646X_ID 0x404C
#define BURST_READ_CMD 0x3E00
const double adis16460_ka_g = 0.25e-3;
const double adis16460_kg = 0.005 * PI / 180.0;
#else
#if IMU_DEVICE != IMU_ADIS16465
#warning "IMU_DEVICE not supported, use default IMU to compiler"
#endif
/* Register for ADIS16465 */
#define MSC_CTRL 0x60
#define SYNC_SCAL 0x62
#define DEC_RATE 0x64
#define FLTR_CTRL 0x5C
#define GLOB_CMD 0x68
#define NULL_CNFG 0x66
#define ADIS1646X_WHO_AM_I 0x72
#define ADIS1646X_ID 0x4051
#define BURST_READ_CMD 0x6800
#define XG_BIAS_LOW  0x40
#define XG_BIAS_HIGH   0x42
#define YG_BIAS_LOW 0x44
#define YG_BIAS_HIGH 0x46
#define ZG_BIAS_LOW 0x48
#define ZG_BIAS_HIGH 0x4A
#define XA_BIAS_LOW 0x4C
#define XA_BIAS_HIGH 0x4E
#define YA_BIAS_LOW 0x50
#define YA_BIAS_HIGH 0x52
#define ZA_BIAS_LOW 0x54
#define ZA_BIAS_HIGH 0x56

/*for ADIS164601BMLZ*/
const double adis16460_ka_g = 0.25e-3;
/*Kg=160*/
const double adis16460_kg = 1.0 / (160.0) * PI / 180.0;
#endif

/*#define SPI_CS_DOWN()  HAL_GPIO_WritePin(SPI5_CS_GPIO_Port,SPI5_CS_Pin,GPIO_PIN_RESET)
#define AdisSpiCsUp();  HAL_GPIO_WritePin(SPI5_CS_GPIO_Port,SPI5_CS_Pin,GPIO_PIN_SET)*/
//#define HSPI hspi5

/*bsp for ADIS16460*/
void AdisSpiCsDown(AdisHandleDef *dev) {
  HAL_GPIO_WritePin(dev->cs_port, dev->cs_pin, GPIO_PIN_RESET);
}
void AdisSpiCsUp(AdisHandleDef *dev) {
  HAL_GPIO_WritePin(dev->cs_port, dev->cs_pin, GPIO_PIN_SET);
}

void SpiWriteReg(AdisHandleDef *dev, uint8_t addr, uint8_t cmd) {
  uint8_t dat[2];
  AdisSpiCsDown(dev);
  dat[1] = addr | 0x80u;
  dat[0] = cmd;
  HAL_SPI_Transmit(dev->hspi, (uint8_t *)dat, 1, 0xFFFF);
  AdisSpiCsUp(dev);
  trace(Info, "write command:0X%4X\n", *(uint16_t *)&dat);
}
uint16_t DevAdisReadId(AdisHandleDef *dev) {
  uint8_t dat[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
  uint8_t reg[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
  AdisSpiCsDown(dev);
  dat[1] = ADIS1646X_WHO_AM_I;
  HAL_StatusTypeDef state = HAL_SPI_Transmit(dev->hspi, dat, 1, 0xfff);
  state |= HAL_SPI_Receive(dev->hspi, (uint8_t *)(reg), 2, 0xfff);
  if (state != HAL_OK) {
	trace(Info, "Receive data from ADIS1646x failed %d\n", state);
	Error_Handler();
  }
  AdisSpiCsUp(dev);
  trace(Info, "1REG:%X %X %X %X\n", reg[0], reg[1], reg[2], reg[3]);
  trace(Info, "2REG:%X %X %X %X\n", reg[4], reg[5], reg[6], reg[7]);
//  return (uint16_t)reg[2] << 8u | reg[0]; //for 465;
  return (uint16_t)reg[0] << 8u | reg[3];
}

/*一个寄存器两个字节*/
uint16_t SpiReadReg(AdisHandleDef *dev,uint8_t addr) {
  uint8_t dat[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
  uint8_t reg[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
  AdisSpiCsDown(dev);
  dat[1] = addr;
  HAL_StatusTypeDef state = HAL_SPI_Transmit(dev->hspi, dat, 1, 0xfff);
  state |= HAL_SPI_Receive(dev->hspi, (uint8_t *)(reg), 2, 0xfff);
  if (state != HAL_OK) {
	trace(Info, "Receive data from ADIS1646x failed %d\n", state);
	Error_Handler();
  }
  AdisSpiCsUp(dev);
  return (uint16_t)reg[0] << 8u | reg[3];
}

int AdisCheckSum(const uint8_t data[20]) {
  uint16_t sum = 0;
  for (int i = 0; i < 20; i++) {
	sum += data[i];
  }
  return sum == 0;//*(uint16_t *)&data[18];
}
int32_t bias[6];
void Adis1646XReadBias(AdisHandleDef *dev) {
  bias[0] = SpiReadReg(dev,XG_BIAS_LOW);
  dev->delay_ms(1);
  bias[0] += (int32_t)SpiReadReg(dev,XG_BIAS_HIGH) << 16u;
  dev->delay_ms(1);
  bias[1] = SpiReadReg(dev,YG_BIAS_LOW);
  dev->delay_ms(1);
  bias[1] += (int32_t)SpiReadReg(dev,YG_BIAS_HIGH) << 16u;
  dev->delay_ms(1);
  bias[2] = SpiReadReg(dev,ZG_BIAS_LOW);
  dev->delay_ms(1);
  bias[2] += (int32_t)SpiReadReg(dev,ZG_BIAS_HIGH) << 16u;
  dev->delay_ms(1);
  bias[3] = SpiReadReg(dev,XA_BIAS_LOW);
  dev->delay_ms(1);
  bias[3] += (int32_t)SpiReadReg(dev,XA_BIAS_HIGH) << 16u;
  dev->delay_ms(1);
  bias[4] = SpiReadReg(dev,YA_BIAS_LOW);
  dev->delay_ms(1);
  bias[4] += (int32_t)SpiReadReg(dev,YA_BIAS_HIGH) << 16u;
  dev->delay_ms(1);
  bias[5] = SpiReadReg(dev,ZA_BIAS_LOW);
  dev->delay_ms(1);
  bias[5] += (int32_t)SpiReadReg(dev,ZA_BIAS_HIGH) << 16u;
}
/**
 * burst reading
 * @param imu
 * @return
 */
int Adis1646XBurstRead(AdisHandleDef *dev, ImuRawAdi *imu) {
  AdisSpiCsDown(dev);
  uint16_t cmd = BURST_READ_CMD;
  HAL_StatusTypeDef status = HAL_SPI_Transmit(dev->hspi, (uint8_t *)&cmd, 1, 0xffff);
  status |= HAL_SPI_Receive(dev->hspi, (uint8_t *)imu, 10, 0xffff);
  AdisSpiCsUp(dev);
  if (status != HAL_OK) {
	trace(Error, "ADIS burst read failed\n");
	Error_Handler();
  }
  /*checksum*/
  return AdisCheckSum((uint8_t *)imu);
}

void Adis1646XInit(AdisHandleDef *dev) {
  uint16_t reg;
  ImuRawAdi adi;
  do {
	reg = DevAdisReadId(dev);
	trace(Info, "IMU ID = %X\n", reg);
	dev->delay_ms(1000);
  } while (reg != ADIS1646X_ID);
  /*上电自检*/
  SpiWriteReg(dev, GLOB_CMD, 0x04);
  dev->delay_ms(10);
  Adis1646XBurstRead(dev, &adi);
  if (adi.stat != 0) {
    dev->trace("ADIS1640 Self-Test FAILED, Stat = %X\n", adi.stat);
	Error_Handler();
  }
  /*disable sync, set dr to 1 when data ready*/
  SpiWriteReg(dev,MSC_CTRL, 0xC1);
  dev->delay_ms(10);
  reg = SpiReadReg(dev,MSC_CTRL);
  dev->trace("MSC_CTRL = %X\n", reg);
  /*sample rate = 1024/(DEC_RATE +1)*/
  SpiWriteReg(dev,DEC_RATE, 0x0f);/*2048/10 = 204.8Hz*/
  dev->delay_ms(10);
  SpiWriteReg(dev,DEC_RATE + 1, 0x00);/*2048/10 = 204.8Hz*/
  reg = SpiReadReg(dev,DEC_RATE);
  dev->trace("DEC_RATE = %X\n", reg);
  Adis1646XReadBias(dev);
  dev->trace("bias : %ld %ld %ld %ld %ld %ld \n",
		bias[0],
		bias[1],
		bias[2],
		bias[3],
		bias[4],
		bias[5]
  );
//    spi_write_reg(FLTR_CTRL, 4);/*filter*/
}
